#include "CubicSpline2D.h"
#include <chrono>

// Default constructor
CubicSpline2D::CubicSpline2D() = default;
// /输入参考线（道路中心线）的x,y坐标，构造二维三次样条函数，即sx,sy，合成x-s坐标系与y-s坐标系
CubicSpline2D::CubicSpline2D(const std::vector<std::pair<double, double>>& x_y, const std::vector<double> s, 
                                                                    const int vehicle_in_segment_start_index, const int vehicle_in_segment_index)
{
    std::vector<double> x, y;
    for(size_t i = 0; i < x_y.size(); i++){
        x.push_back(x_y[i].first);
        y.push_back(x_y[i].second);
    }
    sx = CubicSpline1D(s, x); 
    sy = CubicSpline1D(s, y);
}

// Calculate the x position along the spline at given s，输入的是曲线长度s,也可以是t
double CubicSpline2D::calc_x(double s)
{
    return sx.calc_der0(s);
}

// Calculate the y position along the spline at given s
double CubicSpline2D::calc_y(double s)
{
    return sy.calc_der0(s);
}


// Calculate the yaw along the spline at given s
double CubicSpline2D::calc_yaw(double s)
{
    double dx = sx.calc_der1(s);
    double dy = sy.calc_der1(s);
    double yaw = atan2(dy, dx);
    return yaw;
}

// Calculate the curvature along the spline at given s
double CubicSpline2D::calc_curvature(double s)
{
    double dx = sx.calc_der1(s);
    double ddx = sx.calc_der2(s);
    double dy = sy.calc_der1(s);
    double ddy = sy.calc_der2(s);
    double kappa = (ddy * dx - ddx * dy) / pow(pow(dx, 2) + pow(dy, 2), 1.5);
    return kappa;
}